/***************************************************************************
 *   Copyright (C) 2008 by Justin L. Boss                                  *
 *   justin@rtechguys.com                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <QtDebug>
#include "guidancesystem.h"

GuidanceSystem::GuidanceSystem()
{
    gsMatrix = new GLfloat[16];
    Arrived = false;
    ObjXDeg= 180.0;				//The Object current X angle in Degrees
    ObjYDeg= 180.0;				//The Object current Y angle in Degrees
    ObjZDeg= 180.0;				//The Object current Z angle in Degrees
    x = 0.0;
    y = 0.0;
    z = 0.0;
    DirIncX= 0.0;
    DirIncY= 0.0;
    DirIncZ= 0.0;
    RotIncX= 0.0;
    RotIncY= 0.0;
    RotIncZ= 0.0;
    ActYDeg= 0.0;
    Agility=0.01;
    glLoadIdentity();
    glGetFloatv (GL_MODELVIEW_MATRIX , gsMatrix);
}

GuidanceSystem::~GuidanceSystem()
{
    delete gsMatrix;
}

void GuidanceSystem::gsBegin (int speed)
{
    glLoadMatrixf (gsMatrix);
    {
        /*if (RotIncZ)
        {
            glRotatef (RotIncZ, 0.0, 0.0, 1.0);
            ObjZDeg += RotIncZ;

            if (ObjZDeg > 360)
                ObjZDeg -= 360;
            if (ObjZDeg < 0)
                ObjZDeg += 360;
        }*/

        if (RotIncY)
        {
            glRotatef (RotIncY, 0.0, 1.0, 0.0);
            ObjYDeg+=RotIncY;

            if (ObjYDeg > 360)
                ObjYDeg -= 360;
            if (ObjYDeg < 0)
                ObjYDeg += 360;
        }

        DirIncX = speed * -0.001;
        if (RotIncX)
        {
            glRotatef (RotIncX, 1.0, 0.0, 0.0);
            ObjXDeg+=RotIncX;

            if (ObjXDeg > 360)
                ObjXDeg -= 360;
            if (ObjXDeg < 0)
                ObjXDeg += 360;
        }
        glTranslatef (DirIncX, DirIncY, DirIncZ);
    }
}


void GuidanceSystem::gsEnd()
{
    glGetFloatv (GL_MODELVIEW_MATRIX , gsMatrix);
    x = gsMatrix[12];
    y = gsMatrix[13];
    z = gsMatrix[14];

    ActYDeg = atan2 (z - dz, dx - x) * RADIAN;

    if (ActYDeg > 360)
        ActYDeg -= 360;
    if (ActYDeg < 0)
        ActYDeg += 360;

    if (ObjYDeg > ActYDeg)
        if ((ObjYDeg - ActYDeg) <= 180)
            RotIncY = (ObjYDeg - ActYDeg) * -Agility;
        else
            RotIncY = (ObjYDeg - ActYDeg) * Agility;
    else
        if ((ActYDeg - ObjYDeg) <= 180)
            RotIncY = (ActYDeg - ObjYDeg) * Agility;
        else
            RotIncY = (ActYDeg - ObjYDeg) * -Agility;

    if ((x > dx-3 && x < dx+3) && (z > dz-3 && z < dz+3))
    {
        Arrived = true;
        //qDebug() << "there" << dx << '\t' << dz;
    }
    else
    {
        Arrived = false;
        //qDebug() << x << '\t' << dx << z << '\t' << dz ;
    }
}

void GuidanceSystem:: SetAgility(GLint agility)
{
    Agility = agility * 0.0001;
}

void GuidanceSystem::SetLocation (GLfloat X, GLfloat Y, GLfloat Z)
{
    glLoadIdentity();
    glTranslatef (X, Y, Z);
    glGetFloatv (GL_MODELVIEW_MATRIX , gsMatrix);
    x = gsMatrix[12];
    y = gsMatrix[13];
    z = gsMatrix[14];
}

void GuidanceSystem::SetDestination (GLint X, GLint Y, GLint Z)
{
    dx = X;
    dy = Y;
    dz = Z;
}
